#pragma once
#include "ros/ros.h"
#include "geometry_msgs/Pose2D.h"
#include "actuator_msgs/actuator_msg.h"
#include "localization_msgs/localization_msg.h"
#include "geometry_msgs/Pose.h"
#include "common_lib/common.h"
#include "control_msgs/control_msg.h"
#include "tf/tf.h"

using namespace std;
using namespace actuator_msgs;
using namespace control_msgs;

class PointModel
{
public:
	PointModel(float x, float y, float theta)
	{
		//设置初始位姿
		x_ = x;
		y_ = y;
		theta_ = theta;
	};

	void MoveModelProcess(float v, float w, float speed_toward)
	{
		static double last_time = ros::Time::now().toSec();
		double now_time = ros::Time::now().toSec();
		double dt = now_time - last_time;
		last_time = now_time;

		//模拟伺服延迟功能，从0变成非零，延迟2秒规划速度
        bool is_not_zero   = fabs(v)<0.00001?false:true;
		bool is_not_zero_w = fabs(w)<0.00001?false:true;
        
		if(start_speed_relay_ton_.Q((is_not_zero||is_not_zero_w),500))
		{

		}else
		{
			v=0;
			w=0;
		}

		//angle_in_world
		float speed_dir = common_cal_.AngleConstraintToPI(theta_ + speed_toward);
		float dx = cos(speed_dir) * v;
		float dy = sin(speed_dir) * v;

		x_ += dx * dt;
		y_ += dy * dt;
		theta_ += w * dt;
		theta_ = common_cal_.AngleConstraintToPI(theta_);

		//设置驱动待外发的车体状态
		angle_speed = w;
		walking_speed = v;
		steer_angle = speed_toward;
	};

	geometry_msgs::Pose GetPose()
	{
		geometry_msgs::Pose pose;
		pose.position.x = x_;
		pose.position.y = y_;
		pose.orientation = tf::createQuaternionMsgFromYaw(theta_);
		return pose;
	}

	void GeSteerInfo(float &walking_speed_, float &angle_speed_, float &steer_angle_)
	{
		walking_speed_ = walking_speed;
		angle_speed_ = angle_speed;
		steer_angle_ = steer_angle;
	}

	void SetPose(float x, float y, float theta)
	{
		x_ = x;
		y_ = y;
		theta_ = theta;
	}
private:
	//定位值
	float x_;
	float y_;
	float theta_;

	//舵轮角度
	float steer_angle;
	//线速度	
	float walking_speed;
	//角速度
	float angle_speed;

	common_lib::Common common_cal_;
	common_lib::TON start_speed_relay_ton_;
};



